package ext.sim;

import context.render.vertexdata.VertexData;
import ext.sim.contact.detect.TreeSphere;
import java.util.ArrayList;
import math.mat3;
import math.mat4;
import static math.mat4.*;
import static math.math.*;
import math.region.Sphere3;
import math.vec2;
import math.vec3;
import static math.vec3.*;
import math.vec3i;
import math.vec4;

public class BodyCuboid extends Body
{
    
    private static final double SPHERE_GEN_DENSITY = 4.0;
    
    
    private static final vec4 NNNh = new vec4(-1, -1, -1, 1.0); // VERTICES IN 3D-HOMOGENOUS UNIT BODY-SPACE.
    private static final vec4 NNPh = new vec4(-1, -1,  1, 1.0);
    private static final vec4 NPNh = new vec4(-1,  1, -1, 1.0);
    private static final vec4 NPPh = new vec4(-1,  1,  1, 1.0);
    private static final vec4 PNNh = new vec4( 1, -1, -1, 1.0);
    private static final vec4 PNPh = new vec4( 1, -1,  1, 1.0);
    private static final vec4 PPNh = new vec4( 1,  1, -1, 1.0);
    private static final vec4 PPPh = new vec4( 1,  1,  1, 1.0);
    
    private static final vec4 XNh = new vec4(-1,  0,  0,  0.0); // FACE NORMALS IN 3D-HOMOGENOUS BODY-SPACE.
    private static final vec4 XPh = new vec4( 1,  0,  0,  0.0);
    private static final vec4 YNh = new vec4( 0, -1,  0,  0.0);
    private static final vec4 YPh = new vec4( 0,  1,  0,  0.0);
    private static final vec4 ZNh = new vec4( 0,  0, -1,  0.0);
    private static final vec4 ZPh = new vec4( 0,  0,  1,  0.0);
    
    
    /** Half-Dimensions of the box. */
    public vec3 mHalfSize;
    
    
    /* Vertices of the cuboid. */
    private vec3 nnn;
    private vec3 nnp;
    private vec3 npn;
    private vec3 npp;
    private vec3 pnn;
    private vec3 pnp;
    private vec3 ppn;
    private vec3 ppp;
    
    /* Normals of the cuboid. */
    private vec3 xn;
    private vec3 xp;
    private vec3 yn;
    private vec3 yp;
    private vec3 zn;
    private vec3 zp;
    
    /** Vertex list of this cuboid. */
    public vec3[] mVerts; // Size = [8].
    
    /** Edge list of this cuboid. */
    public vec3[][] mEdges; // Indexed by [edgeIndex][endpointIndex], size = [12][2].
    
    
    
    public BodyCuboid(vec3 aHalfSize)
    {
        super();
        
        this.mHalfSize = aHalfSize.copy();
        
        this.calcIntertiaTensor();
        
        this.nnn = new vec3(NNNh);
        this.nnp = new vec3(NNPh);
        this.npn = new vec3(NPNh);
        this.npp = new vec3(NPPh);
        this.pnn = new vec3(PNNh);
        this.pnp = new vec3(PNPh);
        this.ppn = new vec3(PPNh);
        this.ppp = new vec3(PPPh);
        
        this.xn = new vec3(XNh);
        this.xp = new vec3(XPh);
        this.yn = new vec3(YNh);
        this.yp = new vec3(YPh);
        this.zn = new vec3(ZNh);
        this.zp = new vec3(ZPh);
        
        this.updateVertsAndNorms();
        
        this.mVerts = new vec3[]
        {
            nnn, nnp, 
            npn, npp, 
            pnn, pnp, 
            ppn, ppp
        };
        
        this.mEdges = new vec3[12][2];
        {
            for (int i = 0; i < this.mEdges.length; i++)
            {
                this.mEdges[i] = new vec3[2];
            }
            this.mEdges[0][0]  = nnn;    this.mEdges[0][1]  = pnn;
            this.mEdges[1][0]  = nnn;    this.mEdges[1][1]  = npn;
            this.mEdges[2][0]  = nnn;    this.mEdges[2][1]  = nnp;
            this.mEdges[3][0]  = ppp;    this.mEdges[3][1]  = npp;
            this.mEdges[4][0]  = ppp;    this.mEdges[4][1]  = pnp;
            this.mEdges[5][0]  = ppp;    this.mEdges[5][1]  = ppn;
            this.mEdges[6][0]  = nnp;    this.mEdges[6][1]  = pnp;
            this.mEdges[7][0]  = npn;    this.mEdges[7][1]  = ppn;
            this.mEdges[8][0]  = npn;    this.mEdges[8][1]  = npp;
            this.mEdges[9][0]  = pnn;    this.mEdges[9][1]  = pnp;
            this.mEdges[10][0] = nnp;    this.mEdges[10][1] = npp;
            this.mEdges[11][0] = pnn;    this.mEdges[11][1] = ppn;
        }
        
        ArrayList<Sphere3> spheres = new ArrayList<>();
        {
            vec3 nSpheresf = mul(aHalfSize, SPHERE_GEN_DENSITY);
            nSpheresf.add(1.0);
            vec3i nSpheres = new vec3i(nSpheresf);
            
            vec3 lowerUnit = new vec3(); // in [-1, 1]^3
            vec3 upperUnit = new vec3(); // in [-1, 1]^3
            for (int k = 0; k <= nSpheres.z; k++)
            {
                lowerUnit.z = (2.0 *  k)      / nSpheres.z - 1.0;
                upperUnit.z = (2.0 * (k + 1)) / nSpheres.z - 1.0;
                for (int j = 0; j <= nSpheres.y; j++)
                {
                    lowerUnit.y = (2.0 *  j)      / nSpheres.y - 1.0;
                    upperUnit.y = (2.0 * (j + 1)) / nSpheres.y - 1.0;
                    for (int i = 0; i <= nSpheres.x; i++)
                    {
                        if (i > 0          && j > 0          && k > 0           && 
                            i < nSpheres.x && j < nSpheres.y && k < nSpheres.z)
                        {
                            continue;
                        }
                        
                        lowerUnit.x = (2.0 *  i)      / nSpheres.x - 1.0;
                        upperUnit.x = (2.0 * (i + 1)) / nSpheres.x - 1.0;
                        
                        vec3 lower = mul(lowerUnit, aHalfSize);
                        vec3 upper = mul(upperUnit, aHalfSize);
                        
                        vec3 center = lower;
                        vec3 half = sub(upper, lower);
                        half.mul(0.5);
                        double radius = max(half.x, half.y, half.z);
                        
                        Sphere3 s = new Sphere3(center, radius);
                        spheres.add(s);
                    }
                }
            }
        } // END spheres DEF.
        this.mTreeSphere = new TreeSphere(vec3.ZERO, spheres);
    }
    
    
    public final void calcIntertiaTensor()
    {
        // Compute the inertia tensor in body-space.
        double massFac = this.mMass / 12.0;
        double sqXSpan = 4.0 * this.mHalfSize.x * this.mHalfSize.x;
        double sqYSpan = 4.0 * this.mHalfSize.y * this.mHalfSize.y;
        double sqZSpan = 4.0 * this.mHalfSize.z * this.mHalfSize.z;
        this.mIbody.set(mat3.ZERO);
        this.mIbody.m[0] = massFac * (sqYSpan + sqZSpan); // Diagonal entries.
        this.mIbody.m[4] = massFac * (sqXSpan + sqZSpan);
        this.mIbody.m[8] = massFac * (sqXSpan + sqYSpan);
        
        // Compute the inverse of the above matrix--easy because it's a diagonal matrix!
        this.mInvIbody.m[0] = 1.0 / this.mIbody.m[0];
        this.mInvIbody.m[4] = 1.0 / this.mIbody.m[4];
        this.mInvIbody.m[8] = 1.0 / this.mIbody.m[8];
    }
    
    public final void updateVertsAndNorms()
    {
        mat4 scale = mat4.scale(new vec4(this.mHalfSize, 1.0));
        mat4 transl = mat4.transl(this.mX);
        mat4 rot = new mat4(this.mR);
        
        mat4 worldFromBody = mul(transl, rot);              // TRANSFORM = TRANSL * ROT.
        mat4 worldFromUnitBody = mul(worldFromBody, scale); // TRANSFORM = TRANSL * ROT * SCALE.
        
        this.nnn.set(worldFromUnitBody.mul(NNNh)); // Drops the w-component in vec3(vec4) cast.
        this.nnp.set(worldFromUnitBody.mul(NNPh));
        this.npn.set(worldFromUnitBody.mul(NPNh));
        this.npp.set(worldFromUnitBody.mul(NPPh));
        this.pnn.set(worldFromUnitBody.mul(PNNh));
        this.pnp.set(worldFromUnitBody.mul(PNPh));
        this.ppn.set(worldFromUnitBody.mul(PPNh));
        this.ppp.set(worldFromUnitBody.mul(PPPh));
        
        this.xn.set(worldFromBody.mul(XNh));
        this.xp.set(worldFromBody.mul(XPh));
        this.yn.set(worldFromBody.mul(YNh));
        this.yp.set(worldFromBody.mul(YPh));
        this.zn.set(worldFromBody.mul(ZNh));
        this.zp.set(worldFromBody.mul(ZPh));
    }
    
    
    public boolean contains(vec3 pt)
    {
        vec3 localPt = this.mInvR.mul(pt);
        return this.distanceSigned(localPt) <= 0.0;
    }
    
    
    
    /** Returns an unsigned distance field for an axis-aligned box centered on the origin with the given dimensions. */
    private double distanceUnsigned(vec3 x)
    {
        vec3 d = sub(abs(x), this.mHalfSize);
        d.clamp(0.0, Double.MAX_VALUE);
        return length(d);
    }
    
    /** Returns a signed distance field for an axis-aligned box centered on the origin with the given dimensions. 
        Defined to be negative within the box. */
    private double distanceSigned(vec3 x)
    {
        vec3 d = sub(abs(x), this.mHalfSize);
        double internal = min(max(d.x, d.y, d.z), 0.0);
        d.clamp(0.0, Double.MAX_VALUE);
        double external = max(length(d), 0.0);
        return internal + external;
    }
    
    
    public void render(VertexData aVertexData)
    {
        final vec2 uvN = new vec2(0.0);
        final vec2 uvP = new vec2(1.0);
        
        this.renderFaceXN(aVertexData, uvN, uvP);
        this.renderFaceXP(aVertexData, uvN, uvP);
        this.renderFaceYN(aVertexData, uvN, uvP);
        this.renderFaceYP(aVertexData, uvN, uvP);
        this.renderFaceZN(aVertexData, uvN, uvP);
        this.renderFaceZP(aVertexData, uvN, uvP);
    }
    
    private void renderFaceXN(VertexData aVertexData, vec2 uvN, vec2 uvP)
    {
        aVertexData.addVert(new double[] { nnn.x, nnn.y, nnn.z, xn.x, xn.y, xn.z, uvN.x, uvN.y }); // X-NEG FACE
        aVertexData.addVert(new double[] { npn.x, npn.y, npn.z, xn.x, xn.y, xn.z, uvP.x, uvN.y });
        aVertexData.addVert(new double[] { npp.x, npp.y, npp.z, xn.x, xn.y, xn.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { npp.x, npp.y, npp.z, xn.x, xn.y, xn.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { nnp.x, nnp.y, nnp.z, xn.x, xn.y, xn.z, uvN.x, uvP.y });
        aVertexData.addVert(new double[] { nnn.x, nnn.y, nnn.z, xn.x, xn.y, xn.z, uvN.x, uvN.y });
    }
    
    private void renderFaceXP(VertexData aVertexData, vec2 uvN, vec2 uvP)
    {
        aVertexData.addVert(new double[] { pnn.x, pnn.y, pnn.z, xp.x, xp.y, xp.z, uvN.x, uvN.y }); // X-POS FACE
        aVertexData.addVert(new double[] { ppp.x, ppp.y, ppp.z, xp.x, xp.y, xp.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { ppn.x, ppn.y, ppn.z, xp.x, xp.y, xp.z, uvP.x, uvN.y });
        aVertexData.addVert(new double[] { ppp.x, ppp.y, ppp.z, xp.x, xp.y, xp.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { pnn.x, pnn.y, pnn.z, xp.x, xp.y, xp.z, uvN.x, uvN.y });
        aVertexData.addVert(new double[] { pnp.x, pnp.y, pnp.z, xp.x, xp.y, xp.z, uvN.x, uvP.y });
    }
    
    private void renderFaceYN(VertexData aVertexData, vec2 uvN, vec2 uvP)
    {
        aVertexData.addVert(new double[] { nnn.x, nnn.y, nnn.z, yn.x, yn.y, yn.z, uvN.x, uvN.y }); // Y-NEG FACE
        aVertexData.addVert(new double[] { nnp.x, nnp.y, nnp.z, yn.x, yn.y, yn.z, uvN.x, uvP.y });
        aVertexData.addVert(new double[] { pnp.x, pnp.y, pnp.z, yn.x, yn.y, yn.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { nnn.x, nnn.y, nnn.z, yn.x, yn.y, yn.z, uvN.x, uvN.y });
        aVertexData.addVert(new double[] { pnp.x, pnp.y, pnp.z, yn.x, yn.y, yn.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { pnn.x, pnn.y, pnn.z, yn.x, yn.y, yn.z, uvP.x, uvN.y });
    }
    
    private void renderFaceYP(VertexData aVertexData, vec2 uvN, vec2 uvP)
    {
        aVertexData.addVert(new double[] { npn.x, npn.y, npn.z, yp.x, yp.y, yp.z, uvN.x, uvN.y }); // Y-POS FACE
        aVertexData.addVert(new double[] { ppp.x, ppp.y, ppp.z, yp.x, yp.y, yp.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { npp.x, npp.y, npp.z, yp.x, yp.y, yp.z, uvN.x, uvP.y });
        aVertexData.addVert(new double[] { npn.x, npn.y, npn.z, yp.x, yp.y, yp.z, uvN.x, uvN.y });
        aVertexData.addVert(new double[] { ppn.x, ppn.y, ppn.z, yp.x, yp.y, yp.z, uvP.x, uvN.y });
        aVertexData.addVert(new double[] { ppp.x, ppp.y, ppp.z, yp.x, yp.y, yp.z, uvP.x, uvP.y });
    }
    
    private void renderFaceZN(VertexData aVertexData, vec2 uvN, vec2 uvP)
    {
        aVertexData.addVert(new double[] { nnn.x, nnn.y, nnn.z, zn.x, zn.y, zn.z, uvN.x, uvN.y }); // Z-NEG FACE
        aVertexData.addVert(new double[] { ppn.x, ppn.y, ppn.z, zn.x, zn.y, zn.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { npn.x, npn.y, npn.z, zn.x, zn.y, zn.z, uvN.x, uvP.y });
        aVertexData.addVert(new double[] { nnn.x, nnn.y, nnn.z, zn.x, zn.y, zn.z, uvN.x, uvN.y });
        aVertexData.addVert(new double[] { pnn.x, pnn.y, pnn.z, zn.x, zn.y, zn.z, uvP.x, uvN.y });
        aVertexData.addVert(new double[] { ppn.x, ppn.y, ppn.z, zn.x, zn.y, zn.z, uvP.x, uvP.y });
    }
    
    private void renderFaceZP(VertexData aVertexData, vec2 uvN, vec2 uvP)
    {
        aVertexData.addVert(new double[] { nnp.x, nnp.y, nnp.z, zp.x, zp.y, zp.z, uvN.x, uvN.y }); // Z-POS FACE
        aVertexData.addVert(new double[] { ppp.x, ppp.y, ppp.z, zp.x, zp.y, zp.z, uvP.x, uvP.y });
        aVertexData.addVert(new double[] { pnp.x, pnp.y, pnp.z, zp.x, zp.y, zp.z, uvP.x, uvN.y });
        aVertexData.addVert(new double[] { nnp.x, nnp.y, nnp.z, zp.x, zp.y, zp.z, uvN.x, uvN.y });
        aVertexData.addVert(new double[] { npp.x, npp.y, npp.z, zp.x, zp.y, zp.z, uvN.x, uvP.y });
        aVertexData.addVert(new double[] { ppp.x, ppp.y, ppp.z, zp.x, zp.y, zp.z, uvP.x, uvP.y });
    }
    
    
}
